# -*- encoding: UTF-8 -*- 

"""
  This script gets the signal from the front microphone of Nao and calculates the rms power on it
  It requires numpy
""" 

from naoqi import *
import time
import numpy as np

class SoundProcessingModule(ALModule):
     
    def __init__( self, strName, robotIP,threshold):
        
        #self.pythonBroker = ALBroker("pythonBrokerSoundProcessing","0.0.0.0",9999, robotIP,9559)
        
        
        ALModule.__init__( self, strName);
        self.robotIP = robotIP
        self.thresholdForRMS = threshold
        self.BIND_PYTHON( strName, "processRemote")
        self.ALAudioDevice = ALProxy("ALAudioDevice", robotIP, 9559)
        self.isProcessingDone = False
        self.micFront = []
        
        #for writing raw data to file
        #self.outputFile = open("output_testi.txt", "wb")
    
    def closeBroker(self):
        self.pythonBroker.shutdown()
    

    def startProcessing(self):
        # ask for the front microphone signal sampled at 16kHz
        # if you want the 4 channels call setClientPreferences(self.getName(), 48000, 0, 0)
        self.isProcessingDone = False
        
        try:
            self.ALAudioDevice.setClientPreferences(self.getName(), 16000, 3, 0)
            self.ALAudioDevice.subscribe(self.getName())
        
            while self.isProcessingDone == False:
                time.sleep(0.1)
         
            self.ALAudioDevice.unsubscribe(self.getName())
            
            return True
        
        except KeyboardInterrupt:
            print "Interrupted by user, shutting down"
            return False
        
        except Exception as e:
            print "Audio sound processing error ", e
            return False
        
        #close output file
        #self.outputFile.close() 
  
    def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, inputBuffer):
        self.micFront=self.convertStr2SignedInt(inputBuffer)
        
        
        #compute the rms level on front mic
        rmsMicFront = self.calcRMSLevel(self.micFront)
        #print "rms level mic front = " + str(rmsMicFront)
        if rmsMicFront > self.thresholdForRMS:
            self.isProcessingDone = True

    
    def calcRMSLevel(self,data) :
        rms = 20 * np.log10( np.sqrt( np.sum( np.power(data,2) / len(data)  )))
        return rms
     
    def convertStr2SignedInt(self, data) :
        """This function takes a string containing 16 bits little endian sound samples as input and returns a vector containing the 16 bits sound samples values converted between -1 and 1"""
        signedData=[]
        ind=0;
        for i in range (0,len(data)/2) :
            signedData.append(ord(data[ind])+ord(data[ind+1])*256)
            ind=ind+2
        
        for i in range (0,len(signedData)) :
            if signedData[i]>=32768 :
                signedData[i]=signedData[i]-65536
        
        for i in range (0,len(signedData)) :
            signedData[i]=signedData[i]/32768.0
    
        return signedData
    
if __name__ == '__main__':
    IP= "10.100.45.87"
    # Creation of a new Python Broker
    pythonBroker = ALBroker("pythonBroker","0.0.0.0",9999, IP,9559)
    
    MySoundProcessingModule = SoundProcessingModule("MySoundProcessingModule", IP, -20)
    MySoundProcessingModule.startProcessing()
    
    pythonBroker.shutdown()
